package projects.ufam.filters;

import java.awt.GridLayout;
import java.util.Random;
import java.util.Vector;

import org.jfree.chart.ChartFactory;
import org.jfree.chart.ChartPanel;
import org.jfree.chart.JFreeChart;
import org.jfree.chart.plot.PlotOrientation;
import org.jfree.chart.renderer.xy.XYLineAndShapeRenderer;
import org.jfree.data.xy.XYDataset;
import org.jfree.data.xy.XYSeries;
import org.jfree.data.xy.XYSeriesCollection;
import org.jfree.ui.ApplicationFrame;

import ema.Matrix;

public class KalmanFilterTest {

	private static final double STANDARD_DEVIATION = 100;
	private static final double MEAN = 0;
	private Vector<String> data;

	public KalmanFilterTest() {
    	try {
    		data = new Vector<String>();
    		Random rand = new Random(  );
    		 
            double a = 0; //(m/s2)
            double T = 0.1; //(10/s)
            double v = 0, s = 0;
            double em = 0, ee = 0;

            
            KalmanFilter kalman = new KalmanFilter(2, 1, 1); //KalmanFilter kalman = new KalmanFilter(2, 2, 1);
            double[][] transicao =   { { 1, T },
            		                   { 0, 1 } };
            double[][] controle = { {T*T/2},
                                    { T   } };

            kalman.setError_cov_post( Matrix.identity( kalman.getError_cov_post() ));
            kalman.setTransition_matrix(transicao);
            kalman.setControl_matrix( controle );

            int x = 0;
            double vc = 0, vca = 0;
            for ( x=0; x<100; x++) {
            	double u = (vc - vca)/T;
            	System.out.println(v + "\t" + vc + "\t" + a + "\t" + u);
            	
            	//Previsão
            	double estimated[][] = kalman.predict( new double[][] { {a} } );

            	//Deslocamento e Velocidade Real
            	/*if (x%5==0)*/ a = rand.nextFloat()*100;
            	v = v + a*T;
            	s = s + v*T + a*T*T/2; 

            	//Medição do deslocamento e velocidade
            	double sm = s + rand.nextGaussian( );

            	 //Correção
            	double corrected[][] = kalman.correct( new double[][] { {sm} } );
            	vca = vc;
            	vc = corrected[1][0];
            	//System.out.println(v + "\t" + vc);
            	
            	//Acumulando erros
            	em += Math.pow( s - sm, 2);
            	ee += Math.pow( s - estimated[0][0], 2);
            	
            	//Saída
                String saida = s + "\t" + sm + "\t" + estimated[0][0] + "\t" + corrected[0][0] + "\t" + 
 			                   v + "\t" + v  + "\t" + estimated[1][0] + "\t" + corrected[1][0] + "\n";
                data.add(saida);
            }
            System.out.println("Erro da medição = " + (em/x) );
            System.out.println("Erro da estimação = " + (ee/x) );
        } catch (Exception ex) {
            System.out.println(ex.getMessage());
        }    	
	}
	
	public String getDataAt( int index ) {
		return data.elementAt(index);
	}
	
	public int dataSize() {
		return data.size();
	}

    public static void main(final String[] args) {
    	KalmanFilterTest kalmanDemo = new KalmanFilterTest();
        KalmanDemoGraph g = new KalmanDemoGraph(kalmanDemo);
        g.setSize(800, 600);
        g.setLocationRelativeTo(null);
        g.setVisible(true);
        g.setExtendedState(javax.swing.JFrame.MAXIMIZED_BOTH);
    }
}

class KalmanDemoGraph extends ApplicationFrame {

	private static final long serialVersionUID = 5116751992250232186L;
	private KalmanFilterTest kalmanDemo;

    public KalmanDemoGraph( KalmanFilterTest kalmanDemo ) {
        super("Gráfico");
        this.kalmanDemo = kalmanDemo;
        JFreeChart chartDeslocamento = createChartDeslocamento( createDatasetDeslocamento() );
        JFreeChart chartVelocidade = createChartVelocidade( createDatasetVelocidade() );
        ChartPanel chartPanelDeslocamento = new ChartPanel( chartDeslocamento );
        ChartPanel chartPanelVelocidade = new ChartPanel( chartVelocidade );
        this.setLayout( new GridLayout(2, 1) );
        this.getContentPane().add(chartPanelDeslocamento);
        this.getContentPane().add(chartPanelVelocidade);
    }
    
    private XYDataset createDatasetDeslocamento() {
    	try {
    		XYSeries real = new XYSeries("Real");
    		XYSeries medido = new XYSeries("Medido");
    		XYSeries estimado = new XYSeries("Estimado");
    		String line = null;
    		for( int t=0; t<kalmanDemo.dataSize(); t++) {
    			line = kalmanDemo.getDataAt(t);
    			String[] dados = line.split("\t");
    			real.add(      t, Double.valueOf( dados[0] ) );
    			medido.add(    t, Double.valueOf( dados[1] ) );
    			estimado.add(  t, Double.valueOf( dados[2] ) );
    		} 
    		XYSeriesCollection dataset = new XYSeriesCollection();
    		dataset.addSeries(real);
    		dataset.addSeries(estimado);
    		dataset.addSeries(medido);
    		return dataset;
    	} catch (Exception e) {
    		System.out.println(e.getMessage());
    	}
    	return null;
    }
    
    private XYDataset createDatasetVelocidade() {
    	try {
    		XYSeries real = new XYSeries("Real");
    		XYSeries estimado = new XYSeries("Estimada");
    		String line = null;
    		for ( int t=0; t<kalmanDemo.dataSize(); t++) {
    			line = kalmanDemo.getDataAt(t);
    			String[] dados = line.split("\t");
    			real.add(      t, Double.valueOf( dados[4] ) );
    			estimado.add(  t, Double.valueOf( dados[6] ) );
    			t++;
    		} 
    		XYSeriesCollection dataset = new XYSeriesCollection();
    		dataset.addSeries(real);
    		dataset.addSeries(estimado);
    		return dataset;
    	} catch (Exception e) {
    		System.out.println(e.getMessage());
    	}
    	return null;
    }
    
    private JFreeChart createChartDeslocamento(final XYDataset dataset) {
        JFreeChart chart = ChartFactory.createXYLineChart("Deslocamento - Filtro de Kalman", "Tempo", "Deslocamento", dataset, PlotOrientation.VERTICAL, true, true, false );
        chart.getXYPlot().setRenderer(new XYLineAndShapeRenderer());
        chart.setBackgroundPaint(java.awt.Color.white);
        chart.getPlot().setBackgroundPaint(java.awt.Color.white);
        chart.getXYPlot().setDomainGridlinePaint(java.awt.Color.black);
        chart.getXYPlot().setRangeGridlinePaint(java.awt.Color.black); 
        return chart;
    }
    
    private JFreeChart createChartVelocidade( XYDataset dataset ) {
        JFreeChart chart = ChartFactory.createXYLineChart("Velocidade - Filtro de Kalman", "Tempo", "Velocidade", dataset, PlotOrientation.VERTICAL, true, true, false );
        chart.getXYPlot().setRenderer(new XYLineAndShapeRenderer());
        chart.setBackgroundPaint(java.awt.Color.white);
        chart.getPlot().setBackgroundPaint(java.awt.Color.white);
        chart.getXYPlot().setDomainGridlinePaint(java.awt.Color.black);
        chart.getXYPlot().setRangeGridlinePaint(java.awt.Color.black); 
        return chart;
    }

}
